#include "speedUp.h"
#include "C_code_Log.h"
#include "control.h"
#include "motor.h"
#include "camera.h"
#include "Mydelay.h"
#include "distance.h"
#include "main.h"
struct SpeedUpInfo
{
    int32_t end;  // 加速目标值,距离或时间
    int32_t half;  // 目标值的一半,用于判断是否到达目标值的一半
    int32_t start;  // 开始时的时间/距离
    uint8_t accelerateBaseOn;  // 加速基准,0:距离,1:时间，也表示加速目标值和加速参数的具体含义
    int16_t startSpeed;  // 开始时的速度,PWM值
    int16_t nowSpeed;  // 当前需要设置的速度,PWM值
    uint8_t flag;  // 标志位,0:正在加速，1:加速完成，2：正在减速，3：减速完成
    int32_t upFinish;  // 加速完成的时间/加速完成时已走过距离,用来减速的基准
}speedUpInfo;
/**
 * @brief 获取加速过程中的速度
 * 
 * @return int16_t 速度值，单位：PWM值，分正负
 * @note 此函数用于在加速巡线方法中获取加速过程中的速度
 */
int16_t getSpeedOfAccelerate(){
  return speedUpInfo.nowSpeed;
}
uint8_t accelerateBaseOnDistance(int16_t distance,int16_t acceleration,uint16_t maxSpeed);
uint8_t accelerateBaseAuto(int16_t distance , int acceleration , uint16_t maxSpeed);
/**
 * @brief 根据距离加速
 * 
 * @param distance 加速距离，单位：mm
 * @return uint8_t 
 */
uint8_t Speed_commonMethod(int16_t distance) {
  if (distance == 0) return 1;
  static uint8_t flag = 0;
  //static enum {start, speedUp, adjust} ENUM_flag = start;
  static FindLine save;
  static int32_t startEncode;
  static int32_t adjustDistance;
  static uint8_t stableNum = 0;
  //先正常走一小段（走到巡正为止），再做加速，调整好姿态
  if(flag == 0){
    startEncode=gl_motor.encoderDistance;//记录开始的编码器值
    stableNum = 0;
    flag = 1;
  }
  if(flag == 1){
#define ALLOW_ERR 8
    if(inRange(getOldLineRank(),80-ALLOW_ERR,80+ALLOW_ERR)){
      LOG_INFO("当前rank为%d",getOldLineRank());
      stableNum++;
    }
#undef ALLOW_ERR
    else{
      stableNum = 0;
    }
    //如果在稳定的过程中，走的距离大于要加速的距离，就直接返回
    adjustDistance=((int32_t)gl_motor.encoderDistance-(int32_t)startEncode)/ENCODE_OF_ONE_mm;
    if(distance<adjustDistance){
      flag = 0;
      return 1;
    }
  }
  if(flag == 1&&stableNum>=8)
  {
    adjustDistance=(gl_motor.encoderDistance-startEncode)/ENCODE_OF_ONE_mm;
    if (distance>adjustDistance) {//如果调整的距离小于要加速的距离，就继续加速
      flag = 2;
      save = gl_controlCenter.findLineWay;
      gl_controlCenter.findLineWay = FL_speedUp;
      gl_controlCenter.findLineFlag = 0;
    } else {
      flag = 0;
      return 1;
    }
  }
  if (flag == 2) {
    uint8_t result = accelerateBaseOnDistance(distance-adjustDistance, 300, 6500);
    if (result == 1) {
      flag = 0;
      gl_controlCenter.findLineWay = save;
      gl_controlCenter.findLineFlag = 0;
      return 1;
    }
  }
  return 0;
}
/**
 * @brief 根据距离加速,通过摄像头自动退出加速
 * 
 * @param distance 加速距离，单位：mm
 * @return uint8_t 
 */
uint8_t Speed_commonMethodAuto(int16_t distance) {
  if (distance == 0) return 1;
  static uint8_t flag = 0;
  //static enum {start, speedUp, adjust} ENUM_flag = start;
  static FindLine save;
  static int32_t startEncode;
  static int32_t adjustDistance;
  static uint8_t stableNum = 0;
  //先正常走一小段（走到巡正为止），再做加速，调整好姿态
  if(flag == 0){
    startEncode=gl_motor.encoderDistance;//记录开始的编码器值
    stableNum = 0;
    flag = 1;
  }
  if(flag == 1){
#define ALLOW_ERR 8
    if(inRange(getOldLineRank(),80-ALLOW_ERR,80+ALLOW_ERR)){
      LOG_INFO("当前rank为%d",getOldLineRank());
      stableNum++;
    }
#undef ALLOW_ERR
    else{
      stableNum = 0;
    }
    //如果在稳定的过程中，走的距离大于要加速的距离，就直接返回
    adjustDistance=((int32_t)gl_motor.encoderDistance-(int32_t)startEncode)/ENCODE_OF_ONE_mm;
    if(distance<adjustDistance){
      flag = 0;
      return 1;
    }
  }
  if(flag == 1&&stableNum>=8)
  {
    adjustDistance=(gl_motor.encoderDistance-startEncode)/ENCODE_OF_ONE_mm;
    if (distance>adjustDistance) {//如果调整的距离小于要加速的距离，就继续加速
      flag = 2;
      save = gl_controlCenter.findLineWay;
      gl_controlCenter.findLineWay = FL_speedUp;
      gl_controlCenter.findLineFlag = 0;
    } else {
      flag = 0;
      return 1;
    }
  }
  if (flag == 2) {
    uint8_t result = accelerateBaseAuto(distance-adjustDistance, 300, 6500);
    if (result == 1) {
      flag = 0;
      gl_controlCenter.findLineWay = save;
      gl_controlCenter.findLineFlag = 0;
      return 1;
    }
  }
  return 0;
}
/**
 * @brief 根据距离后退加速
 * 
 * @param distance 加速距离，单位：mm
 * @return uint8_t 
 */
uint8_t Speed_commonMethod_Back(int16_t distance) {
  if(distance==0) return 1;
  static uint8_t flag = 0;
  static FindLine save;
  if(flag == 0){//先切换为后退的巡线方法
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    save = gl_controlCenter.findLineWay;
    if (save != FL_Reversing) {
      gl_controlCenter.findLineWay = FL_Reversing;
      gl_controlCenter.findLineFlag = 0;
    }
    flag = 1;
  }
  //先后退一段时间，等稳定后再加速
  if(flag == 1 && myDelay((uint32_t)Speed_commonMethod_Back,400) == true){
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    gl_controlCenter.findLineWay = FL_speedUp_Back;
    gl_controlCenter.findLineFlag = 0;
    flag = 2;
  }
  if(flag == 2){
    //后退距离，后退加速度（负数），后退最大速度（正数）
    uint8_t result=accelerateBaseOnDistance(-distance,-200,4500);
    if(result==1){
      flag = 0;
      gl_controlCenter.findLineWay = save;
      gl_controlCenter.findLineFlag = 0;
      return 1;
    }
  }
  return 0;
}

/**
 * @brief 根据距离直接加速
 * 
 * @param distance 加速距离，单位：mm
 * @return uint8_t 
 */
uint8_t Speed_directMethod(int16_t distance) {
  if (distance == 0) return 1;
  static uint8_t flag = 0;
  //static enum {start, speedUp, adjust} ENUM_flag = start;
  static FindLine save;
  static int32_t startEncode;
  static int32_t adjustDistance;
  static uint8_t stableNum = 0;
  //先正常走一小段（走到巡正为止），再做加速，调整好姿态
  if(flag == 0){
    startEncode=gl_motor.encoderDistance;//记录开始的编码器值
    stableNum = 0;
    flag = 1;
  }
  if(flag == 1){
#define ALLOW_ERR 5
    if(inRange(getOldLineRank(),80-ALLOW_ERR,80+ALLOW_ERR)){
      LOG_INFO("当前rank为%d",getOldLineRank());
      stableNum++;
    }
#undef ALLOW_ERR
    else{
      stableNum = 0;
    }
    //如果在稳定的过程中，走的距离大于要加速的距离，就直接返回
    adjustDistance=((int32_t)gl_motor.encoderDistance-(int32_t)startEncode)/ENCODE_OF_ONE_mm;
    if(distance<adjustDistance){
      flag = 0;
      return 1;
    }
  }
  if(flag == 1&&stableNum>=13)
  {
    adjustDistance=(gl_motor.encoderDistance-startEncode)/ENCODE_OF_ONE_mm;
    if (distance>adjustDistance) {//如果调整的距离小于要加速的距离，就继续加速
      flag = 2;
      save = gl_controlCenter.findLineWay;
      gl_controlCenter.findLineWay = FL_speedBest;
      gl_controlCenter.findLineFlag = 0;
    } else {
      flag = 0;
      return 1;
    }
  }
  if (flag == 2) {
    uint8_t result = MyDistance((uint32_t)(Speed_directMethod),distance-adjustDistance);
    if (result == 1) {
      flag = 0;
      gl_controlCenter.findLineWay = save;
      gl_controlCenter.findLineFlag = 0;
      return 1;
    }
  }
  return 0;
}
/**
 * @brief 根据距离加速的基础方法
 * 
 * @param distance 加速的距离，单位：mm，可以是正数也可以是负数，正数表示前进，负数表示后退
 * @param acceleration 加速度，前进时为正数，后退时为负数，单位为 PWM值/50ms
 * @param maxSpeed 最大速度，正数，后退时就取速度的绝对值
 * @return uint8_t 1：加速完成，0：加速未完成
 * @note 自动处理加速的过程，包括加速，匀速，减速，先加速到最大速度，然后减速到原来的速度
 * @note 如果总距离较短，可能不会到达最大速度，会在前一半路程加速，后一半路程减速
 * @note 存在问题：加速完成的判断是用PWM值是否大于最大PWM值来判断的，而PWM值并不是实际的速度，所以会有误差\
 * 解决方式：用编码器获取的速度（motor.speed）的差是否小于某个值来判断是否加速完成，要考虑到加速度较小的时候这个值可能会很小，所以小加速度就不用这种方法
 * （问题未解决，解决请删除这个注释）
 */
 uint8_t accelerateBaseOnDistance(int16_t distance,int16_t acceleration,uint16_t maxSpeed) {
  static uint8_t flag = 0;
  static uint32_t runTime=0;
  uint32_t nowTime=getGlobalTime();
  //减速距离和加速距离的比例
  #define RATIO_OF_DECELERATE_DISTANCE_AND_ACCELERATE 2.0f
  if(flag == 0)
  {
    flag = 1;
    //计算距离目标值
    speedUpInfo.end = distance * ENCODE_OF_ONE_mm+gl_motor.encoderDistance;
    LOG_INFO("目标的编码器值:%d",speedUpInfo.end);
    LOG_INFO("现在的编码器值:%d",gl_motor.encoderDistance);
    speedUpInfo.start = gl_motor.encoderDistance;
    //speedUpInfo.half = speedUpInfo.end - distance / 2 * ENCODE_OF_ONE_mm;
    speedUpInfo.half = speedUpInfo.start + distance /(RATIO_OF_DECELERATE_DISTANCE_AND_ACCELERATE + 1) * ENCODE_OF_ONE_mm;
    speedUpInfo.accelerateBaseOn = 0;
    speedUpInfo.startSpeed = (getLeftMotorPWM()+getRightMotorPWM())/2;
    speedUpInfo.nowSpeed = speedUpInfo.startSpeed;
    speedUpInfo.flag = 0;//正在加速
    LOG_INFO("startSpeed:%d",speedUpInfo.startSpeed);
    // //更改巡线方法
    // gl_controlCenter.findLineWay=FL_speedUp;
    // gl_controlCenter.findLineFlag=0;
    //更新时间
    runTime=nowTime;
  }
  if(runTime<=nowTime)
  {
    runTime=nowTime+50;//50ms执行一次加速
    switch (speedUpInfo.flag)
    {
      case 0:
      //当前速度小于最大速度，加速
      if(abs16(speedUpInfo.nowSpeed) <= maxSpeed){
        speedUpInfo.nowSpeed += acceleration;
        if(abs16(speedUpInfo.nowSpeed) >= maxSpeed){
          speedUpInfo.nowSpeed = limit(speedUpInfo.nowSpeed,-maxSpeed,maxSpeed);
          speedUpInfo.flag = 1;//加速完成,进入匀速
          speedUpInfo.upFinish = gl_motor.encoderDistance - speedUpInfo.start ;
          LOG_INFO("加速距离upFinish:%d",speedUpInfo.upFinish);
        }
      }
      //走过的距离大于目标距离的一半，减速
      if (!inRange(gl_motor.encoderDistance,speedUpInfo.start,speedUpInfo.half)){
        speedUpInfo.flag = 2;//加速完成，马上减速，因为已经走过了一半
        LOG_INFO("未达到最大速度,distance:%d",gl_motor.encoderDistance);
      }
      break;
      case 1:
      //剩下的距离小于加速的距离，减速
      if(!inRange(gl_motor.encoderDistance,speedUpInfo.start,(speedUpInfo.end-speedUpInfo.upFinish*RATIO_OF_DECELERATE_DISTANCE_AND_ACCELERATE))){
        speedUpInfo.flag = 2;//开始减速
        LOG_INFO("开始减速时的distance:%d",gl_motor.encoderDistance);
      }
      break;
      case 2:
      //当前速度大于开始速度，减速
      if(abs16(speedUpInfo.nowSpeed) >= abs16(speedUpInfo.startSpeed)){
        speedUpInfo.nowSpeed -= (1.5f*acceleration);//减速的速度是加速的2倍
        //减速完成
        if(inRange(speedUpInfo.startSpeed,speedUpInfo.nowSpeed,speedUpInfo.nowSpeed+(2*acceleration))){
          speedUpInfo.nowSpeed = speedUpInfo.startSpeed;
          speedUpInfo.flag = 3;//减速完成
        }
      }
      //走过的距离大于目标距离，回到原来的速度
      if (!inRange(gl_motor.encoderDistance,speedUpInfo.start,speedUpInfo.end) ) {
        speedUpInfo.nowSpeed = speedUpInfo.startSpeed;
        speedUpInfo.flag = 3;//减速完成
      }
    }
    if(speedUpInfo.flag == 3)
    {
      LOG_INFO("加速完成时的distance:%d",gl_motor.encoderDistance);
      LOG_INFO("加速距离：%dmm",(int)((gl_motor.encoderDistance-speedUpInfo.start)/ENCODE_OF_ONE_mm));
      //加速完成
      flag=0;
      return 1;
    }
  }
  return 0;
#undef RATIO_OF_DECELERATE_DISTANCE_AND_ACCELERATE
}
// 寻找自动加速时，应该发送的指令
CameraCmd_t seekCameraCmd(void)
{
  // 过完障碍之后
  if(gl_controlCenter.runState->F_RoadBlockState == MY_EXIT)
  {
    // 如果有直接冲到障碍前的特征，那就是障碍
    if(gl_controlCenter.sectionInfo->seekNodeWay == SEEK_NONE &&
       gl_controlCenter.sectionInfo->carParkWay == NoPark &&
       gl_controlCenter.sectionInfo->rotateWay == HN_Rotate)
    {
      return CAMERA_CMD_GET_DISTANCE_ROADBLOCK;
    }
    // 如果要上平台，那也是障碍
    else if (gl_controlCenter.sectionInfo->carParkWay == PARK_Platform ||
             gl_controlCenter.sectionInfo->carParkWay == PARK_Platform_ONE )
    {
      return CAMERA_CMD_GET_DISTANCE_ROADBLOCK;
    }
    // 其他都是找点
    else
    {
      return CAMERA_CMD_GET_DISTANCE_POINT;
    }
  }
  // 过完障碍之前
  else
  {
    // 如果有障碍
    if(gl_controlCenter.sectionInfo->roadBlockWay != FREE)
    {
      return CAMERA_CMD_GET_DISTANCE_ROADBLOCK;
    }
    // 没有障碍，但是有冲到障碍的特征
    else if (gl_controlCenter.sectionInfo->seekNodeWay == SEEK_NONE &&
            gl_controlCenter.sectionInfo->carParkWay == NoPark &&
            gl_controlCenter.sectionInfo->rotateWay == HN_Rotate)
    {
      return CAMERA_CMD_GET_DISTANCE_ROADBLOCK;
    }
    // 如果要上平台，那也是障碍
    else if (gl_controlCenter.sectionInfo->carParkWay == PARK_Platform ||
             gl_controlCenter.sectionInfo->carParkWay == PARK_Platform_ONE )
    {
      return CAMERA_CMD_GET_DISTANCE_ROADBLOCK;
    }
    // 其他都是找点
    else
    {
      return CAMERA_CMD_GET_DISTANCE_POINT;
    }
  }
  return CAMERA_CMD_GET_DISTANCE_POINT;
}
//根据前方路障或节点来自动减速，需要一个大概的距离，可以稍微大一点，
//检测到前方一定距离有障碍，可以根据最高速度一定时，加减速的距离与加速度成反比
//根据加速时的加速度和加速时走过的距离，计算出一个减速度，然后根据减速度来减速
uint8_t accelerateBaseAuto(int16_t distance , int acceleration , uint16_t maxSpeed){
  static uint8_t flag = 0;
  static uint32_t runTime=0;
  static uint8_t autoFlag=0;//0:未获取到减速距离，1:获取到减速距离
  uint32_t speedupInterval = 0;
  static CameraCmd_t camera_cmd;
  uint32_t nowTime=getGlobalTime();
  //减速距离和加速距离的比例
  #define RATIO_OF_DECELERATE_DISTANCE_AND_ACCELERATE 2.0f
  if(flag == 0)
  {
    if(distance<=0) {
      LOG_ERROR("自动减速功能不能用于后退");
      return 1;
    }
    autoFlag=0;
    flag = 1;
    //计算距离目标值
    speedUpInfo.end = distance * ENCODE_OF_ONE_mm+gl_motor.encoderDistance;
    LOG_INFO("目标的编码器值:%d",speedUpInfo.end);
    LOG_INFO("现在的编码器值:%d",gl_motor.encoderDistance);
    speedUpInfo.start = gl_motor.encoderDistance;
    //speedUpInfo.half = speedUpInfo.end - distance / 2 * ENCODE_OF_ONE_mm;
    speedUpInfo.half = speedUpInfo.start + distance /(RATIO_OF_DECELERATE_DISTANCE_AND_ACCELERATE + 1) * ENCODE_OF_ONE_mm;
    speedUpInfo.accelerateBaseOn = 0;
    speedUpInfo.startSpeed = (getLeftMotorPWM()+getRightMotorPWM())/2;
    speedUpInfo.nowSpeed = speedUpInfo.startSpeed;
    speedUpInfo.flag = 0;//正在加速
    LOG_INFO("startSpeed:%d",speedUpInfo.startSpeed);
    // //更改巡线方法
    // gl_controlCenter.findLineWay=FL_speedUp;
    // gl_controlCenter.findLineFlag=0;
    //更新时间
    runTime=nowTime;
  }
  if(runTime<=nowTime)
  {
    // runTime=nowTime+50;//50ms执行一次加速
    speedupInterval = nowTime - runTime;
    switch (speedUpInfo.flag)
    {
      case 0:
      //当前速度小于最大速度，加速
      if(abs16(speedUpInfo.nowSpeed) <= maxSpeed){
        speedUpInfo.nowSpeed += acceleration * speedupInterval/50;
        if(abs16(speedUpInfo.nowSpeed) >= maxSpeed){
          speedUpInfo.nowSpeed = limit(speedUpInfo.nowSpeed,-maxSpeed,maxSpeed);
          speedUpInfo.flag = 1;//加速完成,进入匀速
          speedUpInfo.upFinish = gl_motor.encoderDistance - speedUpInfo.start ;
          LOG_INFO("加速距离upFinish:%d",speedUpInfo.upFinish);
        }
      }
      //走过的距离大于目标距离的一半，减速
      if (!inRange(gl_motor.encoderDistance,speedUpInfo.start,speedUpInfo.half)){
        speedUpInfo.flag = 2;//加速完成，马上减速，因为已经走过了一半
        camera_cmd = seekCameraCmd();
        camera_SendCommand(camera_cmd);
        LOG_INFO("未达到最大速度,distance:%d",gl_motor.encoderDistance);
        speedUpInfo.upFinish = gl_motor.encoderDistance - speedUpInfo.start ;
      }
      break;
      case 1:
      //剩下的距离小于加速的距离，减速
      if(!inRange(gl_motor.encoderDistance,speedUpInfo.start,(speedUpInfo.end-speedUpInfo.upFinish*RATIO_OF_DECELERATE_DISTANCE_AND_ACCELERATE))){
        speedUpInfo.flag = 2;//开始减速
        camera_cmd = seekCameraCmd();
        camera_SendCommand(camera_cmd);
        LOG_INFO("开始减速时的distance:%d",gl_motor.encoderDistance);
        HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
      }
      break;
      case 2:
      {
      extern int32_t getRoadBlockDistance(CameraCmd_t cmd);
      //获取到减速距离
      if((speedUpInfo.end-gl_motor.encoderDistance)/ENCODE_OF_ONE_mm <1000){
      int32_t roadBlockDistance=getRoadBlockDistance(camera_cmd);
      static int newAcceleration=0;
      static int32_t oldEndDis; 
      if(roadBlockDistance!=1){
        autoFlag=1;
        HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
        LOG_INFO("获取到减速距离:%d",roadBlockDistance);
        oldEndDis=speedUpInfo.end;
        speedUpInfo.end=gl_motor.encoderDistance+roadBlockDistance*ENCODE_OF_ONE_mm;
//错误：正确应该为newAcceleration=acceleration(原加速度)*distance(原来剩下的距离，为end-nowEncoder)/roadBlockDistance(现在剩下的距离)
        newAcceleration = (speedUpInfo.upFinish *
                          RATIO_OF_DECELERATE_DISTANCE_AND_ACCELERATE) *
                          acceleration / (roadBlockDistance * ENCODE_OF_ONE_mm);
      }
      if(autoFlag==1){
        acceleration=newAcceleration;
      }
      }
      //当前速度大于开始速度，减速
      if(abs16(speedUpInfo.nowSpeed) >= abs16(speedUpInfo.startSpeed)){
        speedUpInfo.nowSpeed -= (1.5f*acceleration* speedupInterval/50);//减速的速度是加速的2倍
        //减速完成
        if(inRange(speedUpInfo.startSpeed,speedUpInfo.nowSpeed,speedUpInfo.nowSpeed+(2*acceleration))){
          LOG_INFO("111");
          speedUpInfo.nowSpeed = speedUpInfo.startSpeed;
          speedUpInfo.flag = 3;//减速完成
        }
      }
      //走过的距离大于目标距离，回到原来的速度
      if (!inRange(gl_motor.encoderDistance,speedUpInfo.start,speedUpInfo.end) ) {
        LOG_INFO("222");
        speedUpInfo.nowSpeed = speedUpInfo.startSpeed;
        speedUpInfo.flag = 3;//减速完成
      }
      }
    }
    if(speedUpInfo.flag == 3)
    {
      LOG_INFO("加速完成时的distance:%d",gl_motor.encoderDistance);
      LOG_INFO("加速距离：%dmm",(int)((gl_motor.encoderDistance-speedUpInfo.start)/ENCODE_OF_ONE_mm));
      //加速完成
      flag=0;
      return 1;
    }
    runTime = nowTime;
  }
  return 0;
#undef RATIO_OF_DECELERATE_DISTANCE_AND_ACCELERATE
}

/// @brief 加速任务回调函数
/// @param control 控制台
/// @note 在完成加速任务后调用，若路障状态为退出，则将加速任务状态设置为完成，将寻找节点任务状态置为准备就绪
/// @note 若路障状态为阻塞，则将加速任务状态设置为阻塞，将路障状态置为准备就绪，为路障后的加速任务做准备
static void speedUpCallback(ControlCenterTypeDef *control) {
    if(control->runState->F_RoadBlockState == MY_EXIT)
    {
        control->runState->speedUpState = MY_EXIT;
        control->runState->seekNodeState = MY_READY;
    }
    else if(control->runState->F_RoadBlockState == MY_BLOCKED)
    {
        control->runState->speedUpState = MY_BLOCKED;
        control->runState->F_RoadBlockState = MY_READY;
    }
} 
/**
 * @brief 占用时间
 * 
 * @param time 
 * @return uint8_t 
 */
uint8_t accelerate_Time(uint16_t time){
  if(myDelay((uint32_t)accelerate_Time,time)==true){
    return 1;
  }
  return 0;
}
/// @brief  加速任务处理函数
/// @param control 控制台
/// @note 在加速任务状态为准备就绪时调用，根据加速任务的类型执行不同的加速任务
/// @note 加速的参数分为两个，一个是路障前的加速参数，一个是路障后的加速参数，通过路障状态来判断调用哪个
/// @note 加速任务完成后调用加速任务回调函数
void speedUpHandle_Task(ControlCenterTypeDef *control) {
  // 如果加速任务状态为准备就绪 并且 路障状态为退出，执行路障后的加速任务
  if (control->runState->speedUpState == MY_READY&&control->runState->F_RoadBlockState == MY_EXIT) {
    // 根据加速任务的类型执行不同的加速任务，sectionInfo是当前路段的信息的指针，指向地图
    switch (control->sectionInfo->speedUpWay) {
      case NOSpeed:
        speedUpCallback(control);
        break;
      case COMMON_SPEED:
        if (1 == Speed_commonMethod(control->sectionInfo->speedUpParam_After)) {
          speedUpCallback(control);
        }
        break;
      case COMMON_Back_SPEED:
        if (1 == Speed_commonMethod_Back(control->sectionInfo->speedUpParam_After)) {
          speedUpCallback(control);
        }
        break;
      case NO_SPEED_TIME:
        if(1 == accelerate_Time(control->sectionInfo->speedUpParam_After)){
            speedUpCallback(control);
        }
        break;
      case AUTO_SPEED:
        if(1 == Speed_commonMethodAuto(control->sectionInfo->speedUpParam_After)){
            speedUpCallback(control);
        }
        break;
      case DIRECT_SPEED:
        if(1 == Speed_directMethod(control->sectionInfo->speedUpParam_After)){
            speedUpCallback(control);
        }
        break;
        default:
          speedUpCallback(control);
        break;
    }
  }
  // 如果加速任务状态为准备就绪 并且 路障状态为阻塞，执行路障前的加速任务
  else if(control->runState->speedUpState == MY_READY && control->runState->F_RoadBlockState == MY_BLOCKED)
  {
    switch (control->sectionInfo->speedUpWay) {
      case NOSpeed:
        speedUpCallback(control);
        break;
      case COMMON_SPEED:
        if (1 == Speed_commonMethod(control->sectionInfo->speedUpParam_Before)) {
          speedUpCallback(control);
        }
        break;
      case NO_SPEED_TIME:
        if(1 == accelerate_Time(control->sectionInfo->speedUpParam_Before)){
            speedUpCallback(control);
        }
        break;
      case AUTO_SPEED:
        if(1 == Speed_commonMethodAuto(control->sectionInfo->speedUpParam_Before)){
            speedUpCallback(control);
        }
        break;
      case DIRECT_SPEED:
        if(1 == Speed_directMethod(control->sectionInfo->speedUpParam_Before)){
            speedUpCallback(control);
        }
        break;
      default:
        speedUpCallback(control);
        break;
    }
  }
}